#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <pthread.h>
#include <unistd.h>
#include "robot.h"

#define DEFAULT_DEVICE "/dev/rfcomm0"
#define BUMPER_CHECK_RATE 10 //Hz (must be > 1)

time_t start_time;

void *check_bumpers(void *param) {
	int rate;
	char bumper;
	time_t stamp;
	
	rate = *((int*)param);
	if(rate <= 1) {
		rate = 2;
	}
	while(1) {
		robot_bumpers(&bumper, &stamp);
		printf("bumbers: ");
		if(bumper < 0 || bumper > 3) {
			printf("not valid data\n");
		}
		printf("left=%s   ", ((bumper & 2) == 2) ? "yes" : "no");
		printf("right=%s   ", ((bumper & 1) == 1) ? "yes" : "no");
		printf("time=%d\n", (int) difftime(stamp, start_time));
		usleep(1000000/rate);
	}
}
	

void *interface_function(void *param);


int main(int argc, char **argv) {
	char *device = DEFAULT_DEVICE;
	pthread_t bumper_thread;
	pthread_t interface_thread;

	start_time = time(NULL);

	if(argc > 1) {
		device = argv[1];
	}

	printf("attempting to connect to %s\n", device);

	if(robot_init(argv[1]) < 0) {
		fprintf(stderr, "could not connect to the robot\n");
		exit(1);
	}
	printf("connected\n");

	int rate = BUMPER_CHECK_RATE;
	//pthread_create(&bumper_thread, NULL, check_bumpers, (void*)&rate);
	pthread_create(&interface_thread, NULL, interface_function, NULL);
	check_bumpers(&rate);

}

void *interface_function(void *param) {
	char c;
	char bumper;
	time_t stamp;
	while(1) {
	scanf("%c", &c);
	if(c == 'q') {
		robot_close();
		exit(0);
	}
	else if(c == 'f') {
		robot_forward();
	}
	else if(c == 'b') {
		robot_backward();
	}
	else if(c == 'l') {
		robot_left();
	}
	else if(c == 'r') {
		robot_right();
	}
	else if(c == 's') {
		robot_stop();
	}
	else if(c == 't') {
		robot_bumpers(&bumper, &stamp);
		printf("bumbers: ");
		if(bumper < 0 || bumper > 3) {
			printf("not valid data\n");
		}
		printf("left=%s   ", ((bumper & 2) == 2) ? "yes" : "no");
		printf("right=%s   ", ((bumper & 1) == 1) ? "yes" : "no");
		printf("time=%d\n", (int) difftime(stamp, start_time));
		}
	}
}

